/*----------------------------------------------------------------------------*/
/* Welcome to FRC Programming!                                                */
/*                                                                            */
/* Please take a look at the ReadMe.txt file in this source before diving     */
/* into the code.  It'll explain how it's all setup in the beginning.         */
/*----------------------------------------------------------------------------*/

/*
 * Please remember to look through the other files in this package, as well.
 * All of the camera operations are contained in Camera2012.java, which does
 * "extend" the Robot2012 class.  That means that it can add onto the functions
 * here.  For example, while this file tells the robot how to drive during
 * teleoperation, the Camera2012 class also gives it instructions on dealing
 * with camera commands during teleoperation.
 * 
 * Settings.java is an "interface"... it doesn't give commands like this file,
 * it only holds variables which this class can use.  For example, there is a
 * variable in Settings.java called "debugTesting".  If it is 1, things may
 * act differently here and in Camera2012.java.  You'll notice these features
 * by the line "if(debugTesting == 1) {".
 */

/*
 * This set of code is packaged together under the title "frc2012".  This
 * particular class (Robot2012) requires that we import several things.
 * 
 * import edu.wpi.first.wpilibj.*; imports almost everything in the library,
 * except for its subfolders (like .camera).  This includes all of the library
 * files necessary for using a joystick, driverStation, etc.
 */

package edu.wpi.first.wpilibj.frc2012;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.image.*;
import java.lang.Math;

/*
 * Here is the code itself.  Robot2012 is the main "class" of our code, which
 * other things (at the moment, Camera2012) will be based on.  This class
 * "extends" something called IterativeRobot - that's a class set up by the
 * good people at WPI that acts as a base for performing basic robot functions.
 * "IterativeRobot" includes several functions, or "voids", which we will
 * override.
 * 
 * Robot2012 also "implements" Settings - Settings is an "interface", as
 * mentioned above.  By implementing it, we can use the variables it contains,
 * just as by extending "IterativeRobot" we can use its functions.
 */

public class Robot2012 extends IterativeRobot implements Settings {

/*
 * Here are the variables that Robot2012 will use.  We will both declare, and
 * set, these variables here.
 * 
 * RobotDrive is the drive base.  WPI's library creates the variable type
 * "RobotDrive" as something that controls four motor controllers (in our case,
 * Jaguar motor controllers).  We call our RobotDrive "driveTrain", and we
 * set it as a new RobotDrive with Jaguars on ports 1,2,3,4.  This means that
 * the Jaguars connected to ports 1,2,3,4 on the digital sidecar will be used
 * by the program to drive around.
 * 
 * Later on, we'll say "driveTrain.tankDrive(joyLeft,joyRight);" which tells
 * the robot to use ports 1,2 based on the left Joystick and 3,4 based on the
 * right Joystick.
 */
    
    RobotDrive driveTrain = new RobotDrive(1,2,6,7);
    Jaguar downer = new Jaguar(3);
    Jaguar intake = new Jaguar(4);
    Jaguar shooter = new Jaguar(5);
    Jaguar turret = new Jaguar(8);
    
/*
 * The driver's station is broken up into two parts - the station itself, which
 * connects to the joysticks, and the LCD which can be used to output data.
 * Calling "getInstance()" connects the robot and the driver's station together.
 */
    
    DriverStation driverStation = DriverStation.getInstance();
    DriverStationLCD dsLCD = DriverStationLCD.getInstance();

/*
 * Here are our three Joysticks.  The numbers 1,2,3 tell the robot and the
 * driver's station which USB ports the sticks will be plugged into.
 */
    
    Joystick joyLeft = new Joystick(1);
    Joystick joyRight = new Joystick(2);
    Joystick joyExtra = new Joystick(3);
    
/*
 * One very useful sensor is the encoder.  We use Quadrature Encoders, which
 * require two digital In/Out ports per sensor.  That's why there are two
 * numbers used in the creation of the encoder.  The true / false statement
 * says whether the robot needs to reverse the direction of the encoder (it does
 * for the right one, because it is oriented in opposite direction).
 */
    
    Encoder encoderLeft = new Encoder(3,4,false);
    Encoder encoderRight = new Encoder(5,6,true);
    
/*
 * We aren't quite ready for the gyro and the accelerometer yet, because I'm
 * not sure how to properly call them.
 */
    
    Gyro gyro = new Gyro(1);
    Accelerometer accel = new Accelerometer(2);
    
    CriteriaCollection criteria = new CriteriaCollection();
    public AxisCamera camera = AxisCamera.getInstance();
    
/*
 * Very soon, there will be a feature that turns off unnecessary sensors and
 * driveTrain operation when the camera is being used to aim and shoot a
 * basketball.  The cameraOp variable will be used for that: 0 if the robot is
 * in normal operation, 1 if those items need to be turned off.
 * 
 * The variable teleopPacketCount will be used later.
 */
    
    int operationState = 1;
    int teleopPacketCount = 0;
    double encoderLeftValue = 0;
    double encoderRightValue = 0;
    double gyroValue = 0;
    double accelValue = 0;
    double shooterPower = 0;
    int shooterLock = 0;
    
    double best_score = 0;
    int best_x = 0;
    int best_y = 0;

/*
 * The following presents the various states of the robot.  Each of the modes,
 * disabled / autonomous / teleoperation have an initialization, periodic, and
 * continuous.  These functions come from "IterativeRobot", a class by WPI...
 * We can override them and add in our own code to supplement theirs.
 * 
 * Periodic is the most important - it is what we use during 
 * teleoperation to send data back and forth between the robot and driver's
 * station.  It also sends data to the motors.
 * 
 * Initialization is a good time to set servos, reset the camera (see the
 * class Camera2012 in Camera2012.java for this), and print a message to the
 * driver's station saying that the robot has correctly switched modes.
 * 
 * Be very careful with continuous functions - they are done as often as
 * possible, even moreso than the periodic functions... if you try to do too
 * much there, it will use up all of the robot's CPU!
 * 
 * printLCD() is a custom-build function ("void") which you can find further
 * down in the code.  I created it to make writing something to the driver's
 * station LCD easier.
 */
    
    public void robotInit() {
        printLCD(1,"Initializing...     ");
        resetRobot(1,1,1);
        camera.writeCompression(0);
        camera.writeResolution(AxisCamera.ResolutionT.k640x480);
        
        criteria.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        criteria.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
    }
    public void disabledInit() {
        printLCD(1,"Disabled            ");
    }
    public void disabledPeriodic() {}
    public void disabledContinuous() {}

/*
 * Here are the autonomous functions.  Autonomous periodic is tricky!  If you
 * want to drive forward for a few seconds, you can't just say "drive forward"
 * because it'll start to try to drive forward every few milliseconds.
 * 
 * Timers will be your friend, along with "if" sensor statements - more on this
 * later.
 */

    public void autonomousInit() {
        printLCD(1,"Autonomous          ");
        resetRobot(1,1,1);
    }
    public void autonomousPeriodic() {}
    public void autonomousContinuous() {}
    
/*
 * Teleoperation is the real meat of the program.  You need to be able to drive
 * the robot, which is accomplished using driveTrain.tankDrive();
 * 
 * There are many ways to use tankDrive(), but we tend to use the following:
 *      driveTrain.tankDrive(left joystick , right joystick);
 * 
 * Every time teleopPeriodic is called, an integer called "teleopPacketCount"
 * is increased by one.  This will be useful later.
 * 
 * Remember, the periodic function is called every few milliseconds.  Try not
 * to do anything too CPU-intensive every single packet.  You can break up
 * sensor reports by using the packet counts.  The "%" function gives the
 * remainder of a number, so for example, if teleopPacketCount is divisible by
 * 4, meaning if(teleopPacketCount % 4 == 0), it can perform a CPU-intensive
 * operation.  In this way, it only does it for every fourth packet.  You can
 * even split up various things by using %4 == 0, == 1, etc.
 */

    public void teleopInit() {
        printLCD(1,"Teleoperation       ");
        printLCD(2,"1 Normal Drive Mode ");
        resetRobot(1,1,1);
    }
    public void teleopPeriodic() {
        
        if(operationState != 1 && operationState != 2 &&
                operationState != 3) {
            operationState = 1;
        }
        
        if(joyExtra.getRawButton(7) && joyExtra.getRawButton(8)) {
            if(joyExtra.getRawButton(1)) {
                operationState = 1;
                shooterPower = 0;
                shooterLock = 0;
                shooter.set(0);
                printLCD(2,"1 Normal Drive Mode ");
            }
            else if(joyExtra.getRawButton(2)) {
                operationState = 2;
                shooterPower = 0;
                shooterLock = 0;
                shooter.set(0);
                printLCD(2,"2 DriveTrain Off    ");
            }
            else if(joyExtra.getRawButton(3)) {
                operationState = 3;
                shooterPower = 0;
                shooterLock = 0;
                shooter.set(0);
                printLCD(2,"3 Precision Shooting");
            }
        }
        
/******************************************************************************/
        
        if(operationState == 1) {
            
            driveTrain.tankDrive(joyLeft, joyRight);
            
            if(joyLeft.getRawButton(1) || joyRight.getRawButton(1)) {
                shooter.set(1);
            }
            else {
                shooter.set(Math.abs(joyExtra.getRawAxis(4)));
            }
            
      ///////////////
            
            if(joyLeft.getRawButton(4) || joyRight.getRawButton(4)
                    || joyExtra.getRawButton(5)) {
                downer.set(.2);
            }
            else if(joyLeft.getRawButton(5) || joyRight.getRawButton(5)
                    || joyExtra.getRawButton(6)) {
                downer.set(-.2);
            }
            else {
                downer.set(0);
            }
            
      ///////////////
            
            if(joyLeft.getRawButton(3) || joyRight.getRawButton(3)
                    || joyExtra.getRawButton(4)) {
                intake.set(1);
            }
            else if(joyLeft.getRawButton(2) || joyRight.getRawButton(2)
                    || joyExtra.getRawButton(2)) {
                intake.set(-1);
            }
            else {
                intake.set(0);
            }
            
      ////////////////
            
            turret.set(joyExtra.getRawAxis(1));
            
      ////////////////
            
        }
        
/******************************************************************************/
        
        else if(operationState == 2) {
            
     //////////////////
            
            driveTrain.tankDrive(0,0);
            
     //////////////////
            
            shooter.set(Math.max(Math.max(Math.abs(joyLeft.getRawAxis(1)),
                    Math.abs(joyRight.getRawAxis(1))),
                    Math.abs(joyExtra.getRawAxis(4))));
            
     //////////////////
            
            if(joyLeft.getRawButton(3) || joyRight.getRawButton(3)
                    || joyExtra.getRawButton(4)) {
                intake.set(1);
            }
            else if(joyLeft.getRawButton(2) || joyRight.getRawButton(2)
                    || joyExtra.getRawButton(2)) {
                intake.set(-1);
            }
            else {
                intake.set(0);
            }
            
     /////////////////
            
            if(Math.abs(joyLeft.getRawAxis(2)) > Math.abs(joyRight.getRawAxis(2))
                    && Math.abs(joyLeft.getRawAxis(2)) > Math.abs(joyExtra.getRawAxis(1))) {
                turret.set(joyLeft.getRawAxis(2));
            }
            else if(Math.abs(joyRight.getRawAxis(2)) > Math.abs(joyExtra.getRawAxis(1))) {
                turret.set(joyRight.getRawAxis(2));
            }
            else {
                turret.set(joyExtra.getRawAxis(1));
            }
            
            if(joyLeft.getRawButton(4) || joyRight.getRawButton(4)
                    || joyExtra.getRawButton(5)) {
                downer.set(.2);
            }
            else if(joyLeft.getRawButton(5) || joyRight.getRawButton(5)
                    || joyExtra.getRawButton(6)) {
                downer.set(-.2);
            }
            else {
                downer.set(0);
            }
            
        }
        
        else if(operationState == 3) {
            
            driveTrain.tankDrive(0,0);
            
            if(joyLeft.getRawButton(3) || joyRight.getRawButton(3)) {
                intake.set(1);
            }
            else if(joyLeft.getRawButton(2) || joyRight.getRawButton(2)) {
                intake.set(-1);
            }
            else {
                intake.set(0);
            }
            
     ////////////////////////
            
            if(Math.abs(joyLeft.getRawAxis(2)) > .1) {
                if(shooterPower <= 1 && shooterPower >= 0) {
                    shooterPower += (-joyLeft.getRawAxis(2)/500);
                }
            }
            if(shooterPower > 1) {
                shooterPower = 1;
            }
            else if(shooterPower < 0) {
                shooterPower = 0;
            }
            
    ///////////////////////////
            
            if(Math.abs(joyLeft.getRawAxis(1)) > Math.abs(joyRight.getRawAxis(1))) {
                turret.set(joyLeft.getRawAxis(1));
            }
            else {
                turret.set(joyRight.getRawAxis(1));
            }
            
            if(joyLeft.getRawButton(1)) {
                shooter.set(shooterPower);
            }
            else if(joyRight.getRawButton(1)) {
                shooter.set(0);
            }
        }
        
        //Regardless of Operation State:
        
        
        
        if(joyLeft.getRawButton(8) && joyLeft.getRawButton(9)) {
            operationState = 1;
        }
        else if(joyRight.getRawButton(8) && joyRight.getRawButton(9)) {
            operationState = 1;
        }
        
        if(joyLeft.getRawButton(10) || joyRight.getRawButton(10)) {
            findRectangle();
        }
        
        printLCD(3,"Intended Power: "+(int)(100*shooterPower)+"%   ");
        printLCD(4,"Current Power: "+(int)(100*shooter.get())+"%   ");
        printLCD(5,"Chng Mode: hold 7+8");
        printLCD(6,"then press 1,2,3.");
        
        teleopPacketCount++;
        //updateDashboard();
    }
    public void teleopContinuous() {}
    
/*
 * The following is a custom-made function for printing text to the driver's
 * station LCD.  It takes two arguments: first, the line number to print to,
 * and second, the text to print.
 * 
 * What this cannot do, currently, is begin the text at a certain column. If
 * you use the pre-built function, dsLCD.println(), you can set where the
 * text begins (that's the second argument).
 * 
 * I'm unsure how this function will handle it if you try to input a string
 * and numbers, for example printLCD(1, "Encoder Value: " + encoder.getValue());
 * 
 * Updating the LCD does put the new text on the screen - what it does NOT do,
 * however, is take the old text down.  You'll notice that the printLCD() calls
 * above have strings such as "Teleoperation       " ... this is to put white-
 * spaces where the old text may be.  Otherwise, if your new string is too
 * small, the old string will show up after it.  Example: Autonomousion, if you
 * somehow went from Teleoperat(ion) to Autonomous.
 */
    
    public void printLCD(int line, String text) {
        if(line == 1) {
            dsLCD.println(DriverStationLCD.Line.kMain6, 1, text);
        }
        else if(line == 2) {
            dsLCD.println(DriverStationLCD.Line.kUser2, 1, text);
        }
        else if(line == 3) {
            dsLCD.println(DriverStationLCD.Line.kUser3, 1, text);
        }
        else if(line == 4) {
            dsLCD.println(DriverStationLCD.Line.kUser4, 1, text);
        }
        else if(line == 5) {
            dsLCD.println(DriverStationLCD.Line.kUser5, 1, text);
        }
        else if(line == 6) {
            dsLCD.println(DriverStationLCD.Line.kUser6, 1, text);
        }
        dsLCD.updateLCD();
    }
    
    public void resetRobot(int state, int packets, int sensors) {
        if(state == 1) {
            operationState = 1;
        }
        if(packets == 1) {
            teleopPacketCount = 0;
        }
        if(sensors == 1) {
            encoderLeft.reset();
            encoderRight.reset();
            gyro.reset();
            
            encoderLeftValue = 0;
            encoderRightValue = 0;
            gyroValue = 0;
            accelValue = 0;
        }
    }
    
    public void findRectangle() {
        printLCD(6,"Camera Tracking...  ");
        try {
            ColorImage colorImage = camera.getImage();
            BinaryImage thresholdImage =  colorImage.thresholdHSL(hueLow, hueHigh, saturationLow, saturationHigh, luminenceLow, luminenceHigh);
            BinaryImage filterImage = thresholdImage.removeSmallObjects(false, 2);
            BinaryImage convexImage = filterImage.convexHull(false);
            BinaryImage finalImage = convexImage.particleFilter(criteria);
            
            best_score = 0;
            best_x = 0;
            best_y = 0;
            
            ParticleAnalysisReport[] reports = finalImage.getOrderedParticleAnalysisReports();  // get list of results
            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.println(r.toString());
                if(r.particleQuality > best_score) {
                    best_x = r.center_mass_x;
                    best_y = r.center_mass_y;
                }
            }
            System.out.println("Best Scores: " + best_x + " , " + best_y);
            System.out.println(finalImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());
            printLCD(6,"Best: " + best_x + ", " + best_y + "          ");
            
            finalImage.free();
            convexImage.free();
            filterImage.free();
            thresholdImage.free();
            colorImage.free();
        }
        catch (AxisCameraException ex) {
            ex.printStackTrace();
        }
        catch (NIVisionException ex) {
            ex.printStackTrace();
        }
        
    }
    
    
    void updateDashboard() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        
        lowDashData.addCluster();
        {
            lowDashData.addDouble(shooterPower);
            lowDashData.addDouble(encoderLeftValue);
            lowDashData.addDouble(encoderRightValue);
            lowDashData.addDouble(gyroValue);
            lowDashData.addDouble(accelValue);
        }
        lowDashData.finalizeCluster();
        
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    int module = 1;
                    lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                    lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                    lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                    lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                    lowDashData.addCluster();
                    {
                        for (int i = 1; i <= 10; i++) {
                            lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                        }
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

    }
}